#ifndef _BUSINESS_H_
#define _BUSINESS_H_
#include "rads_database.hpp"
#include "ros/ros.h"
#include "std_msgs/String.h"
#include <sstream> // 用于字符串流
#include <vector>  // 用于整数数组
#include <iostream>
#include <sqlite3.h>
#include <ctime>
#include <string>
#include <iomanip>
#include <cstdlib>
#include <sys/wait.h>
#include "ros/time.h" // 用于ros::Time
#include <tf/transform_listener.h>
#include <thread>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <nav_msgs/Path.h>
#include <fstream>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <cstdio>
#include <unistd.h>
#include <math.h>
#include "rads/rads_scmdret.h"
#include "rads/rads_cmd.h"
#include "rads/rads_acks.h"
#include "rads/rads_version.h"
#include "rads/rads_versions.h"
#include "rads/rads_camera.h"
#include "rads/rads_log.h"
#include <chrono>
#include <mutex>
#include <cmath>
#include <stdexcept>
#include <functional>
#include <std_msgs/Float32.h>
#include "nav_msgs/Odometry.h"
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/Imu.h>
#include "serial/serial.h"
#include "HCNetSDK.h"
using namespace std;
ros::Publisher sradsack_pub_;  // 发布响应网络信息（主要用于响应网络指令，上传地图，上传路线点等）话题名《sradsack》
ros::Publisher sradsdrive;     // 发布控制车辆的信息,话题名《sradsdriveackermann》
ros::Publisher initialposepub; // 发布地图重定位,话题名《initialpose》
ros::Publisher pradscamerapub; //发布双光谱摄像头报警信息
// ros::Publisher sradsversionpub;//发布OTA版本，话题名《sradsversion》
// ros::Publisher sradslogpub;// 发布日志话题，话题名《sradslog》
rads::rads_scmdret rads_scmdretdata;
// 全局变量和同步机制
using ThreadFunc_createline = std::function<void(int, int, string)>; // 定义《带图带线路线点》传参
using ThreadFunc_createline_all = std::function<void(int)>;          // 定义《马踏飞燕》传参
using ThreadFunc_createline_goback = std::function<void(int)>;       // 定义《跟推踩点》传参
using ThreadFunc_createline_goback_play = std::function<void(int)>;  // 定义《返航》传参

std::atomic<bool> should_run(false);             // 带图带线路线点
std::atomic<bool> should_run_all(false);         // 马踏飞燕
std::atomic<bool> should_run_goback(false);      // 跟推踩点
std::atomic<bool> should_run_goback_play(false); // 返航
std::atomic<bool> stopRadarThread(false);        // 自动驾驶

std::thread thread_instance_createline;             // 定义《带图带线路线点》线程
std::thread thread_instance_createline_all;         // 定义《马踏飞燕》线程
std::thread thread_instance_createline_goback;      // 定义《跟推踩点》线程
std::thread thread_instance_createline_goback_play; // 定义《返航》线程

ThreadFunc_createline thread_func_createline;                         // 带图带线路线点
ThreadFunc_createline_all thread_func_createline_all;                 // 马踏飞燕
ThreadFunc_createline_goback thread_func_createline_goback;           // 跟推踩点
ThreadFunc_createline_goback_play thread_func_createline_goback_play; // 返航

std::thread startinginit;

int Stop_lidar, Stop_lidar_qian,Stop_lidar_r, Stop_lidar_l, stop_car, getmap_id;
int line_stop_flag = 0;
int key_1, key_2, key_3, key_4, key_5, key_6, key_7, key_8;
int ladar_1, ladar_2, ladar_3, ladar_4, ladar_5, ladar_6, ladar_7, ladar_8;
int FLLR = 0, LFRFLRRR = 0;
int heartbeat = 0;
double MAX_X, MIN_X, MAX_Y, MIN_Y, MIN_Z, HEADING_ERROR;
int STOP_3D_PONIT, RADAR_TEST;
static double last_latitude = 0.0; //双光谱摄像头报警信息
static double last_longitude = 0.0; //双光谱摄像头报警信息
static ros::Time last_alarm_time; //双光谱摄像头报警信息
static bool is_first_alarm = true; //双光谱摄像头报警信息
std::atomic<bool> need_turnaround{true}; 
std::atomic<bool> goback_forward{true};   // 返航true=正向（向终点），false=反向（向起点）
std::atomic<int> current_target_index{0}; // 记录返航当前目标点索引
// 新增：掉头状态枚举
enum TurnaroundState {
    TS_READY,       // 准备掉头
    TS_TURNING,     // 正在掉头
    TS_COMPLETED    // 掉头完成
};
TurnaroundState turnaround_state = TS_READY;
// rtk&&雷达所有数据
typedef struct
{
  int point_count;
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double yaw;
  double matching_rate;
  double longitude;
  double latitude;
  double altitude;
  double heading;
  double groud_speed;
  double posi_flag;
  double head_flag;
  double ext;
  double flag;
  double rtk_x;
  double rtk_y;
  double rtk_z;
  double f_x;
  double f_y;
  double f_z;
  double ld_heading;
  double base_length;
} locate_information;
locate_information lidar_rtk;
// 雷达返航使用的数据
typedef struct
{
  double x;
  double y;
  double z;
  double ox;
  double oy;
  double oz;
  double ow;
  double ld_heading;
} lidar_goback;
lidar_goback lidar_gobackmsg;
// 外装的IMU
typedef struct
{
  double x;
  double y;
  double z;
} imu;
imu imu_msg;
// sradsack应答式发布
typedef struct
{
  string vehicle_id;
  string pro_nema;
  int cmd;
  int cmd_opt;
  int ext1;
  int ext2;
  int ack;
  int map_id;
  int path_id;
  string map_path;
  string progress;
} sradsack_fast;

// 摄像头配置结构体
struct CameraConfig {
    std::string ip;
    std::string username;
    std::string password;
    std::string saveDir;
};


void publishacks(ros::Publisher pub, const sradsack_fast &sradsack_fast_data)
{
  rads::rads_acks msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "sradsack_map"; // or whatever frame_id is appropriate
  msg.pro_nema = sradsack_fast_data.pro_nema;
  msg.cmd = sradsack_fast_data.cmd;
  msg.cmd_opt = sradsack_fast_data.cmd_opt;
  msg.ext1 = sradsack_fast_data.ext1;
  msg.ext2 = sradsack_fast_data.ext2;
  msg.ack = sradsack_fast_data.ack;
  msg.map_path = sradsack_fast_data.map_path;
  msg.map_id = sradsack_fast_data.map_id;
  msg.path_id = sradsack_fast_data.path_id;
  pub.publish(msg);
}
// 计算两个坐标点之间的距离
double distance_count(double aim_x, double aim_y, double pos_x, double pos_y)
{
  double distance = sqrt((aim_x - pos_x) * (aim_x - pos_x) + (aim_y - pos_y) * (aim_y - pos_y));
  return distance;
}

// 重定位
void Repositioning(int map_id)
{
  double minNDT_gl = 70.0; // 匹配度
  geometry_msgs::PoseWithCovarianceStamped pose_msg;
  pose_msg.header.stamp = ros::Time::now();
  pose_msg.header.frame_id = "map";
  pose_msg.pose.pose.position.x = 0;
  pose_msg.pose.pose.position.y = 0;
  pose_msg.pose.pose.position.z = 0;
  pose_msg.pose.pose.orientation.x = 0;
  pose_msg.pose.pose.orientation.y = 0;
  pose_msg.pose.pose.orientation.z = 0;
  pose_msg.pose.pose.orientation.w = 0;
  initialposepub.publish(pose_msg);
}
// 切换地图
bool map_ck(int map_id)
{

  char command[256];
  snprintf(command, sizeof(command), "cd /home/argo/catkin_ws/src/rads-master/shell && ./map_loader.sh %d &", (int)map_id);
  int result = system(command);
  // Repositioning(map_id);
  return result;
}

/*任意点导航 start*/
// 计算两个坐标点之间的距离
double DistanceCount(AxisData &data1, AxisData &data2)
{
  double distance = sqrt(pow((data1.x - data2.x), 2) + pow((data1.y - data2.y), 2));
  return distance;
}
// 创建节点 value点的索引值
ListNode *CreateNode(int value)
{
  return new ListNode(value);
}
// 向前插入节点
void InsertNodeToHead(ListNode *&head, int value)
{
  ListNode *newNode = CreateNode(value);
  newNode->next = head;
  head = newNode;
}
// 打印链表
void PrintList(ListNode *head)
{
  Path_test[0] = 0;
  while (head != nullptr)
  {
    std::cout << head->value << " -> ";
    Path_test[0]++;
    Path_test[Path_test[0]] = head->value;
    head = head->next;
  }
  std::cout << "null" << std::endl;
}
// 输入一个链表，返回int数组，点位总数（第一位）和点位索引（从索引1开始往后表示）
int *CopyList(ListNode *head)
{
  static int copy_data[1000]; // 需要返回的值和点位
  copy_data[0] = 0;
  while (head != nullptr)
  {
    copy_data[0]++;
    copy_data[copy_data[0]] = head->value;
    head = head->next;
  }
  return copy_data;
}
// 输入坐标x，y 匹配最近点的索引
int FindPoint(AxisData &data, std::vector<point_all> &points)
{
  double minDistance = std::numeric_limits<double>::max(); // 初始化一个最大距离用于计算邻近点比较器
  for (auto i = 0; i != points.size(); i++)
  {
    AxisData tmp;
    tmp.x = points[i].x;
    tmp.y = points[i].y;
    double distance = DistanceCount(data, tmp);
    if (distance < minDistance)
    {
      minDistance = distance, Current_NUM = i;
    }
  }
  // path_pub.publish(path);
  return Current_NUM;
}
// 找当前索引点最近距离点
int *FindPointNum(int point_num, std::vector<point_all> &points)
{
  static int point_data[5]; // 需要返回的值和点位
  point_data[0] = 0;
  for (auto i = 0; i != points.size(); i++)
  {

    AxisData aim(Nav_x[point_num], Nav_y[point_num]);
    AxisData pos(Nav_x[i], Nav_y[i]);
    double distance = DistanceCount(aim, pos);
    if (distance < 3.5 && distance > 1.5)
    {
      point_data[0]++;
      point_data[point_data[0]] = i;
    }
  }
  if (point_data[0] > 0)
  {
    Nav_x[point_num] = 99999;
    Nav_y[point_num] = 99999;
  }
  return point_data;
}
// 计算起点和目标点的规划路径
vector<int16_t> PathPlaning(AxisData &curPose, AxisData &aimPose, std::vector<point_all> &points)
{
  int jump_flag = 0;
  int Line_num = 0;
  int Line_count = 1;
  vector<int16_t> res;
  // 起点 给出坐标值，输出该坐标的标号//当前坐标
  int pos_num = FindPoint(curPose, points);
  // 目标点 给出坐标值，输出该坐标的标号//目标
  int aim_num = FindPoint(aimPose, points);

  if (pos_num == aim_num)
  {

    res.push_back(aim_num);
  }
  else
  {

    for (int i = 0; i < 180; ++i)
    {
      lists[i] = nullptr;
    }
    InsertNodeToHead(lists[0], aim_num); // 把30条路线的头插入 第一个点，把初始搜索点插入到第0条线路的第一个
    int count20 = 0;
    //********************         导航算法如下           *************************
    for (auto i = 0; i != points.size(); i++)
    {
      if (jump_flag == 1)
      {
        break;
      }
      int Line_data = Line_count;
      for (int j = 0; j < Line_data; ++j) // 有多少根线，对每根线操作   J 代表第几根线
      {
        count20++;
        if (jump_flag == 1)
        {
          break;
        }

        int *p = FindPointNum(lists[j]->value, points); // 把每个线路的最后一个点位查找附近点数量//    如果复制链表，需要在此处下复制
        if (*(p) == 1)
        {
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
        }
        else if (*(p) == 2)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 1;
        }
        else if (*(p) == 3)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 2;
        }
        else if (*(p) == 4)
        {
          int *R = CopyList(lists[j]);          // 输出链表的个数和数字  lists[j + Line_count - 1] = nullptr;
          InsertNodeToHead(lists[j], *(p + 1)); // 代表的是第几个连表，代表的是插入的数据值
          for (size_t f = *(R); f > 0; f--)
          {
            InsertNodeToHead(lists[Line_count], *(R + f));     // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 1], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
            InsertNodeToHead(lists[Line_count + 2], *(R + f)); // 代表的是第几个连表，代表的是插入的数据值
          }
          InsertNodeToHead(lists[Line_count], *(p + 2));     // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 1], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          InsertNodeToHead(lists[Line_count + 2], *(p + 3)); // 代表的是第几个连表，代表的是插入的数据值
          Line_count = Line_count + 3;
        }

        for (size_t x = 0; x < *p; x++)
        {
          if (*(p + 1 + x) == pos_num)
          {
            jump_flag = 1;
            Line_num = j;
            break;
          }
        }
      }
    }
    if (Line_num >= 0)
    {
      ListNode *head = lists[Line_num];
      // PrintList(head);
      while (head != nullptr)
      {
        res.push_back(head->value);
        head = head->next;
      }
    }
  }

  return res;
}
std::vector<int16_t> getPath_anypoint(double x1, double y1, double x2, double y2, std::vector<point_all> &points)
{
  AxisData currentPosition(x1, y1);
  AxisData targetPosition(x2, y2);
  return PathPlaning(currentPosition, targetPosition, points);
}
/*任意点导航 end*/

// 角度归一化函数
double normalize_angle(double angle)
{
  while (angle < -M_PI)
    angle += 2 * M_PI;
  while (angle >= M_PI)
    angle -= 2 * M_PI;
  return angle;
}

// 吸附点查找函数

int findSnappedWaypoint(const std::vector<car> &pathPoints)
{
  int snapped_index = -1;
  double min_distance = std::numeric_limits<double>::max();
  const double MAX_SEARCH_DISTANCE = 9.0; // 距离阈值
  double distance, point_heading, heading_diff;

  // 排除终点
  for (size_t i = 0; i < pathPoints.size(); ++i)
  {
    const car &point = pathPoints[i];

    // 1. 计算距离
    if (lidar_rtk.matching_rate > 45.0)
    {
      distance = distance_count(lidar_rtk.x, lidar_rtk.y, point.x, point.y);
    }
    else
    {
      distance = distance_count(lidar_rtk.rtk_x, lidar_rtk.rtk_y, point.rtk_x, point.rtk_y);
    }

    // 距离超过阈值则跳过该点
    if (distance > MAX_SEARCH_DISTANCE)
    {
      continue;
    }

    // 2. 计算航向角差（统一计算方式）
    double delta_x, delta_y;
    if (lidar_rtk.matching_rate > 45.0)
    {
      delta_x = point.x - lidar_rtk.x;
      delta_y = point.y - lidar_rtk.y;
      point_heading = atan2(delta_y, delta_x) * 57.3 + HEADING_ERROR; // 弧度转角度
      heading_diff = fabs(normalize_angle(point_heading - lidar_rtk.ld_heading));
    }
    else
    {
      delta_x = point.rtk_x - lidar_rtk.rtk_x;
      delta_y = point.rtk_y - lidar_rtk.rtk_y;
      point_heading = atan2(delta_y, delta_x) * 57.3 + 270; // 弧度转角度并调整坐标系
      heading_diff = fabs(normalize_angle(point_heading - lidar_rtk.heading));
    }

    // 3. 筛选条件：距离最近 + 前方点（航向差 < 90°）
    if (distance < min_distance && heading_diff < 90.0)
    { // 使用角度值比较
      min_distance = distance;
      snapped_index = i;
    }
  }

  // 如果吸附点是终点或无效索引，则重置为 0
  if (snapped_index >= pathPoints.size() - 1)
  {
    snapped_index = 0;
  }

  std::cout << "snapped_index: " << snapped_index << std::endl; 

  return snapped_index;
}
// 激光雷达
double calculate_NDT_JDC(const car &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.x - target_point.x;
  delta_y = lidar_rtk.y - target_point.y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + HEADING_ERROR; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.ld_heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}
// rtk
double rtkcalculate_NDT_JDC(const car &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.rtk_x - target_point.rtk_x;
  delta_y = lidar_rtk.rtk_y - target_point.rtk_y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + 270; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}

// goback激光雷达
double goback_calculate_NDT_JDC(const go_back &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_gobackmsg.x - target_point.x;
  delta_y = lidar_gobackmsg.y - target_point.y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + HEADING_ERROR; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_gobackmsg.ld_heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}

// rtk
double goback_rtkcalculate_NDT_JDC(const go_back &target_point)
{
  // 计算差值
  double delta_x, delta_y, aim_angle, angle_diff;
  delta_x = lidar_rtk.rtk_x - target_point.rtk_x;
  delta_y = lidar_rtk.rtk_y - target_point.rtk_y;
  aim_angle = atan2(delta_x, delta_y) * 57.3 + 270; // 弧度转角度
  if (aim_angle < 360.0)
  {
    aim_angle = aim_angle - 360.0;
  }
  if (aim_angle < 0.0)
  {
    aim_angle = aim_angle + 360.0;
  }

  // 计算角度差（当前朝向与目标角度的差）
  angle_diff = aim_angle - lidar_rtk.heading;
  if (angle_diff > 180.0)
  {
    angle_diff -= 360.0;
  }
  else if (angle_diff < -180.0)
  {
    angle_diff += 360.0;
  }

  return angle_diff;
}
// 定义常量PI
const double PIS = 3.14159265358979323846;
// 将角度转换为弧度
double degreesToRadians(double degrees)
{
  return degrees * PIS / 180.0;
}
// 计算两个天线中间位置的坐标
void calculateMidPoint(double x1, double y1, double yawDegrees, double baseline, double &xMid, double &yMid)
{
  // 将航向角度转换为弧度
  double yawRadians = degreesToRadians(yawDegrees);
  // 计算第二个天线的坐标
  double deltaX = cos(yawRadians) * baseline;
  double deltaY = sin(yawRadians) * baseline;
  double x2 = x1 + deltaX;
  double y2 = y1 + deltaY;
  // 计算中间位置的坐标
  xMid = (x1 + x2) / 2.0;
  yMid = (y1 + y2) / 2.0;
}

void pubdrive(int brake, int gear, int steering, int target_speed)
{
  heartbeat++;
  rads_scmdretdata.heartbeat = heartbeat;
  rads_scmdretdata.len = 25;      // len
  rads_scmdretdata.brake = brake; // 制动值0-100
  rads_scmdretdata.gear = 3;   // 0=P，1=N，2=D，3=R，4=O；
  rads_scmdretdata.steering = steering;
  rads_scmdretdata.target_speed = target_speed; // 速度：单位：KM/h
  rads_scmdretdata.header.stamp = ros::Time::now();
  rads_scmdretdata.header.frame_id = "sradsdriveackermann";
  sradsdrive.publish(rads_scmdretdata);
}
// 计算当前车辆剩余里程，Lidar_msg当前车辆的坐标，data，是所有的点集，startIdx是下个目标点下标
double calculateTotalDistance(int startIdx, const std::vector<car> &pathPoints)
{
  double totalDistance = 0.0;
  if (startIdx >= pathPoints.size())
  {
    return -1.0; // 或者其他表示错误的特殊值
  }
  const car &fpoint = pathPoints[startIdx];

  double fx = fpoint.x; // 将字符串转换为double
  double fy = fpoint.y; // 将字符串转换为double
  totalDistance += distance_count(lidar_rtk.x, lidar_rtk.y, fx, fy);
  for (size_t i = startIdx; i < pathPoints.size() - 1; ++i)
  {
    const car &point1 = pathPoints[i];
    const car &point2 = pathPoints[i + 1];
    double x1 = point1.x; // 将字符串转换为double
    double y1 = point1.y; // 将字符串转换为double
    double x2 = point2.x; // 将字符串转换为double
    double y2 = point2.y; // 将字符串转换为double
    totalDistance += distance_count(x1, y1, x2, y2);
  }
  return totalDistance;
}

void publishSradsackProgress(int i, const std::vector<car> &pathPoints, int speed,int line_id,int map_id)
{
  rads::rads_acks msg;
  msg.header.stamp = ros::Time::now();
  msg.header.frame_id = "sradsack_map";
  msg.ack = 3;
  // Calculate progress percentage
  double newpercentage = 100 - (static_cast<double>(pathPoints.size() - i) / pathPoints.size() * 100.0);
  int newbaifenbi = static_cast<int>(newpercentage + 0.5); // rounded percentage
  // Calculate distance and time
  double totalDistance = calculateTotalDistance(i, pathPoints);
  int newlicheng = static_cast<int>(totalDistance);
  double newspeedInMPS = speed / 10.0 / 3.6;
  double newseconds = totalDistance / newspeedInMPS;
  int newduoshaomiao = static_cast<int>(newseconds);
  // Format progress string
  msg.progress = std::to_string(newbaifenbi) + "," +
                 std::to_string(newduoshaomiao) + "," +
                 std::to_string(newlicheng)+ "," +
                  std::to_string(line_id)+ "," +
                 std::to_string(map_id);
  sradsack_pub_.publish(msg);
}

// 踩点处理
double normalized_heading_diff(double a, double b)
{
  double diff = fabs(a - b);
  return (diff > 180.0) ? 360.0 - diff : diff; // 处理环绕
}


// 计算两点间距离（Haversine公式）
double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
    const double R = 6371000; // 地球半径，单位：米
    double phi1 = lat1 * M_PI / 180.0;
    double phi2 = lat2 * M_PI / 180.0;
    double delta_phi = (lat2 - lat1) * M_PI / 180.0;
    double delta_lambda = (lon2 - lon1) * M_PI / 180.0;
    
    double a = sin(delta_phi/2) * sin(delta_phi/2) +
               cos(phi1) * cos(phi2) *
               sin(delta_lambda/2) * sin(delta_lambda/2);
    double c = 2 * atan2(sqrt(a), sqrt(1-a));
    
    return R * c;
}

int Capture(int img_id)
{
    // 初始化SDK
    NET_DVR_Init();
    
    // 定义两个摄像头的配置
    std::vector<CameraConfig> cameras = {
        {"192.168.1.64", "admin", "uwb!@#qwe", "/home/argo/catkin_ws/src/rads-master/img"},
        {"192.168.1.65", "admin", "ipc12345", "/home/argo/catkin_ws/src/rads-master/reimg"}
    };
    
    bool anySuccess = false;
    
    // 循环处理每个摄像头
    for (const auto& camera : cameras) {
        long lUserID = -1;
        
        // 登录摄像头
        NET_DVR_USER_LOGIN_INFO struLoginInfo = {0};
        NET_DVR_DEVICEINFO_V40 struDeviceInfoV40 = {0};
        struLoginInfo.bUseAsynLogin = false;
        
        struLoginInfo.wPort = 8000;
        memcpy(struLoginInfo.sDeviceAddress, camera.ip.c_str(), std::min(camera.ip.length(), (size_t)NET_DVR_DEV_ADDRESS_MAX_LEN - 1));
        memcpy(struLoginInfo.sUserName, camera.username.c_str(), std::min(camera.username.length(), (size_t)NAME_LEN - 1));
        memcpy(struLoginInfo.sPassword, camera.password.c_str(), std::min(camera.password.length(), (size_t)NAME_LEN - 1));
        
        lUserID = NET_DVR_Login_V40(&struLoginInfo, &struDeviceInfoV40);
        
        if (lUserID < 0) {
            printf("摄像头 %s 登录失败, 错误码: %d\n", camera.ip.c_str(), NET_DVR_GetLastError());
            continue; // 继续尝试下一个摄像头
        }
        
        // 捕获图像
        NET_DVR_JPEGPARA strPicPara = {0};
        strPicPara.wPicQuality = 2;
        strPicPara.wPicSize = 0;
        std::string filepath = camera.saveDir + "/" + std::to_string(img_id) + ".jpeg";
        int iRet = NET_DVR_CaptureJPEGPicture(lUserID, struDeviceInfoV40.struDeviceV30.byStartChan, &strPicPara, const_cast<char*>(filepath.c_str()));
        
        if (!iRet) {
            printf("摄像头 %s 捕获图像失败, 错误码: %d\n", camera.ip.c_str(), NET_DVR_GetLastError());
        } else {
            printf("摄像头 %s 图像保存成功: %s\n", camera.ip.c_str(), filepath.c_str());
            // 添加修改文件权限的代码
            if (chmod(filepath.c_str(), 0777) == 0) {
                printf("成功将文件 %s 权限设置为 777\n", filepath.c_str());
            } else {
                printf("设置文件 %s 权限失败, 错误码: %d\n", filepath.c_str(), errno);
            }
            anySuccess = true; // 至少有一个摄像头成功
        }
        
        // 登出摄像头
        NET_DVR_Logout_V30(lUserID);
    }
    
    // 清理SDK资源
    NET_DVR_Cleanup();
    
    // 只要有一个摄像头成功捕获图像，就返回成功
    return anySuccess ? HPR_OK : HPR_ERROR;
}



//PID
class AnglePIDController {
private:
    // PID参数
    double kp, ki, kd;              
    double max_output, min_output;   
    
    // 控制器状态
    double integral;                
    double prev_error;              
    double prev_derivative;         // 用于微分项滤波
    ros::Time prev_time;            
    
    // 微分项滤波系数 (0.0~1.0，越小滤波越强)
    const double derivative_filter_alpha = 0.2;

public:
    AnglePIDController(double p, double i, double d, double min, double max) 
        : kp(p), ki(i), kd(d), min_output(min), max_output(max),
          integral(0), prev_error(0), prev_derivative(0), 
          prev_time(ros::Time::now()) {}

    // 重置控制器状态
    void reset() {
        integral = 0;
        prev_error = 0;
        prev_derivative = 0;
        prev_time = ros::Time::now();
        ROS_DEBUG("PID controller reset");
    }

    //添加getIntegral
    double getIntegral() const {
        return integral;
    }

    // 计算PID输出 (主函数)
    double calculate(double setpoint, double pv) {
        // === 1. 时间差计算 ===
        ros::Time now = ros::Time::now();
        double dt = (now - prev_time).toSec();
        prev_time = now;
        if (dt <= 0) dt = 0.001;  // 最小时间步长保护

        // === 2. 角度误差标准化 (-180°~180°) ===
        double error = pv - setpoint;  // 关键修改：交换相减顺序
        while (error > 180.0) error -= 360.0;
        while (error < -180.0) error += 360.0;

        // 增加死区处理，例如当误差在±1度范围内时，误差设为0
        if (error < 1 && error > -1) {
            error = 0;
        }

        // === 3. 比例项 ===
        double P = kp * error;

        // === 4. 积分项 (带抗饱和) ===
        integral += error * dt;

        // 计算未限幅的临时输出，用于积分抗饱和判断
        double temp_output = P + (ki * integral) + kd * ((error - prev_error) / dt);

        // 增强积分抗饱和机制：当临时输出达到限幅且误差与输出同方向时，停止积分增长
        bool at_max = (temp_output >= max_output) && (error > 0);
        bool at_min = (temp_output <= min_output) && (error < 0);
        if (at_max || at_min) {
            integral -= error * dt;  // 撤销刚才的积分增长
        }

        // 积分限幅 (简单条件判断实现)
        double max_integral = (ki != 0) ? (max_output * 0.3 / fabs(ki)) : 0;
        if (integral > max_integral) {
            integral = max_integral;
        } else if (integral < -max_integral) {
            integral = -max_integral;
        }
        double I = ki * integral;

        // === 5. 微分项 (带低通滤波) ===
        double raw_derivative = (error - prev_error) / dt;
        double D = kd * (derivative_filter_alpha * raw_derivative + 
                        (1 - derivative_filter_alpha) * prev_derivative);
        prev_derivative = D;
        prev_error = error;

        // === 6. 输出合成与限幅 ===
        double output = P + I + D;
        if (output > max_output) {
            output = max_output;
        } else if (output < min_output) {
            output = min_output;
        }

        ROS_DEBUG_THROTTLE(0.5, 
            "PID Debug: error=%.3f | P=%.3f I=%.3f D=%.3f | output=%.3f",
            error, P, I, D, output);

        return output;
    }

    // 动态调整PID参数
    void setGains(double p, double i, double d) {
        kp = p;
        ki = i;
        kd = d;
        ROS_INFO("PID gains updated: Kp=%.3f Ki=%.3f Kd=%.3f", kp, ki, kd);
    }
};

// 增强版PID (带停车状态管理)
class EnhancedAnglePIDController : public AnglePIDController {
private:
    const int& stop_car_ref;     // 停车信号引用
    const int& stop_lidar_ref;   // 激光雷达停止信号引用
    bool was_stopped;           // 上一次状态

public:
    EnhancedAnglePIDController(double p, double i, double d, double min, double max,
                             const int& stop_car, const int& stop_lidar)
        : AnglePIDController(p, i, d, min, max),
          stop_car_ref(stop_car),
          stop_lidar_ref(stop_lidar),
          was_stopped(false) {}

    double calculate(double setpoint, double pv) {
        // === 1. 停车状态检测 ===
        bool is_stopped = (stop_car_ref != 1) || (stop_lidar_ref == 1);

        // === 2. 状态切换处理 ===
        if (is_stopped) {
            if (!was_stopped) {
                reset();  // 首次进入停车状态时重置控制器
                ROS_WARN("Vehicle stopped! PID reset.");
            }
            was_stopped = true;
            return 0.0;  // 强制输出零
        } 
        else if (was_stopped) {
            reset();     // 从停车恢复时完全重置
            was_stopped = false;
            ROS_WARN("Vehicle resumed from stop.");
        }

        // === 3. 正常PID计算 ===
        double output = AnglePIDController::calculate(setpoint, pv);
        
        // 在主控制循环中打印积分项值（新增）
        ROS_INFO_THROTTLE(0.5, "Integral raw value: %.3f", getIntegral());        
        return output;
    }
};



#endif